import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import time
import os
from datetime import datetime

#设置中文字体
import matplotlib
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
try:
    import platform
    if platform.system() == 'Darwin':  # macOS
        plt.rcParams['font.sans-serif'] = ['Arial Unicode MS', 'Heiti TC']
    elif platform.system() == 'Linux':
        plt.rcParams['font.sans-serif'] = ['DejaVu Sans', 'WenQuanYi Micro Hei']
except:
    pass

# 1. 数据输入 
print("="*60)
print("问题1：FY1以120m/s飞向假目标，1.5s后投弹，3.6s后起爆")
print("="*60)

# 系统参数
PARAMS = {
    # 目标参数
    'fake_target': np.array([0, 0, 0]),        # 假目标位置
    'real_target': np.array([0, 200, 0]),      # 真目标位置
    'target_radius': 7,                         # 真目标半径
    'target_height': 10,                        # 真目标高度
    
    # 导弹参数
    'missile_M1': np.array([20000, 0, 2000]),  # M1初始位置
    'missile_speed': 300,                       # 导弹速度 m/s
    
    # 无人机参数
    'uav_FY1': np.array([17800, 0, 1800]),     # FY1初始位置
    'uav_speed': 120,                           # 给定速度 m/s
    
    # 烟幕参数
    'smoke_radius': 10,                         # 烟幕有效半径 m
    'smoke_sink_speed': 3,                      # 烟幕下沉速度 m/s
    'smoke_duration': 20,                       # 烟幕有效时间 s
    
    # 投放参数
    'launch_time': 1.5,                         # 投放时间 s
    'explosion_delay': 3.6,                     # 起爆延迟 s
    
    # 物理常数
    'gravity': 9.8,                             # 重力加速度 m/s²
    
    # 仿真参数
    'time_step': 0.01,                          # 时间步长 s（更小更精确）
}

# 2. 参数初始化
class SmokeScreenSimulator:
    """烟幕遮蔽仿真器"""
    
    def __init__(self, params):
        self.params = params
        # 计算导弹飞行总时间
        missile_distance = np.linalg.norm(params['missile_M1'] - params['fake_target'])
        self.total_time = missile_distance / params['missile_speed']
        print(f"导弹飞行总时间: {self.total_time:.2f}s")
        
        # 创建输出文件夹
        self.output_dir = f"simulation_output_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
        if not os.path.exists(self.output_dir):
            os.makedirs(self.output_dir)
            print(f"创建输出文件夹: {self.output_dir}")
        
    def calculate_uav_position(self, t):
        """计算无人机在时刻t的位置（等高度飞行）"""
        # 计算水平方向向量（忽略z分量）
        horizontal_target = self.params['fake_target'].copy()
        horizontal_target[2] = self.params['uav_FY1'][2]  # 保持无人机高度
        
        direction = horizontal_target - self.params['uav_FY1']
        horizontal_distance = np.sqrt(direction[0]**2 + direction[1]**2)
        
        if horizontal_distance > 0:
            # 归一化水平方向
            direction = direction / np.linalg.norm(direction)
        else:
            direction = np.array([0, 0, 0])
        
        # 无人机等高度飞行
        position = self.params['uav_FY1'] + direction * self.params['uav_speed'] * t
        return position
    
    def calculate_smoke_release_position(self):
        """计算烟幕弹投放位置"""
        launch_pos = self.calculate_uav_position(self.params['launch_time'])
        print(f"投放位置: ({launch_pos[0]:.1f}, {launch_pos[1]:.1f}, {launch_pos[2]:.1f})")
        return launch_pos
    
    def calculate_smoke_explosion_position(self, launch_pos):
        """
        计算烟幕弹起爆位置
        """
        # 计算无人机的水平速度矢量
        horizontal_target = self.params['fake_target'].copy()
        horizontal_target[2] = self.params['uav_FY1'][2]
        
        direction = horizontal_target - self.params['uav_FY1']
        horizontal_distance = np.sqrt(direction[0]**2 + direction[1]**2)
        
        if horizontal_distance > 0:
            direction_normalized = direction / np.linalg.norm(direction)
            # 无人机的速度矢量（水平）
            uav_velocity = direction_normalized * self.params['uav_speed']
        else:
            uav_velocity = np.array([0, 0, 0])
        
        # 起爆延迟时间
        dt = self.params['explosion_delay']
        
        # 水平位移（继承无人机的水平速度）
        horizontal_displacement = uav_velocity * dt
        
        # 垂直位移（自由落体，初速度为0）
        vertical_displacement = -0.5 * self.params['gravity'] * dt * dt
        
        # 起爆位置
        explosion_pos = launch_pos + horizontal_displacement
        explosion_pos[2] += vertical_displacement  # 只修改z坐标
        
        print(f"起爆位置: ({explosion_pos[0]:.1f}, {explosion_pos[1]:.1f}, {explosion_pos[2]:.1f})")
        return explosion_pos
    
    def calculate_missile_position(self, t):
        """计算导弹在时刻t的位置"""
        # 导弹直线飞向假目标
        direction = self.params['fake_target'] - self.params['missile_M1']
        direction = direction / np.linalg.norm(direction)
        
        position = self.params['missile_M1'] + direction * self.params['missile_speed'] * t
        return position
    
    def calculate_smoke_center(self, t, explosion_pos, explosion_time):
        """计算烟幕中心在时刻t的位置"""
        if t < explosion_time:
            return None  # 还未起爆
        
        if t > explosion_time + self.params['smoke_duration']:
            return None  # 烟幕已失效
        
        # 烟幕下沉
        dt = t - explosion_time
        smoke_center = explosion_pos.copy()
        smoke_center[2] -= self.params['smoke_sink_speed'] * dt  # z方向下沉
        
        return smoke_center
    
    def ray_sphere_intersection(self, ray_start, ray_end, sphere_center, sphere_radius):
        """
        光线-球体相交检测（核心算法）
        使用向量方法判断线段是否与球体相交
        """
        if sphere_center is None:
            return False
        
        # 线段方向向量
        d = ray_end - ray_start
        # 起点到球心的向量
        f = ray_start - sphere_center
        
        # 二次方程系数 at² + bt + c = 0
        a = np.dot(d, d)
        if a == 0:  # 避免除零错误
            return False
            
        b = 2 * np.dot(f, d)
        c = np.dot(f, f) - sphere_radius * sphere_radius
        
        # 判别式
        discriminant = b * b - 4 * a * c
        
        if discriminant < 0:
            return False  # 无交点
        
        # 计算交点参数t
        sqrt_discriminant = np.sqrt(discriminant)
        t1 = (-b - sqrt_discriminant) / (2 * a)
        t2 = (-b + sqrt_discriminant) / (2 * a)
        
        # 检查交点是否在线段上（t∈[0,1]）
        return (0 <= t1 <= 1) or (0 <= t2 <= 1) or (t1 < 0 and t2 > 1)
    
    def simulate_obstruction(self):
        """模拟遮蔽过程（光线追踪主算法）"""
        # 计算关键位置
        launch_pos = self.calculate_smoke_release_position()
        explosion_pos = self.calculate_smoke_explosion_position(launch_pos)
        explosion_time = self.params['launch_time'] + self.params['explosion_delay']
        
        print(f"起爆时间: {explosion_time:.2f}s")
        
        # 时间离散化
        time_points = np.arange(0, self.total_time, self.params['time_step'])
        obstruction_status = np.zeros_like(time_points)
        
        # 记录调试信息
        debug_info = []
        
        # 模拟
        print("\n开始光线追踪计算...")
        start_compute = time.time()
        
        for i, t in enumerate(time_points):
            # 获取当前位置
            missile_pos = self.calculate_missile_position(t)
            smoke_center = self.calculate_smoke_center(t, explosion_pos, explosion_time)
            
            # 光线追踪判定遮蔽
            if smoke_center is not None:
                # 判断导弹到真目标的视线是否被烟幕阻挡
                is_blocked = self.ray_sphere_intersection(
                    missile_pos, 
                    self.params['real_target'],
                    smoke_center,
                    self.params['smoke_radius']
                )
                obstruction_status[i] = 1 if is_blocked else 0
                
                # 记录关键时刻的调试信息
                if i % 100 == 0:  # 每100个时间步记录一次
                    missile_to_target_dist = np.linalg.norm(missile_pos - self.params['real_target'])
                    smoke_to_line_dist = self.calculate_point_to_line_distance(
                        smoke_center, missile_pos, self.params['real_target']
                    )
                    debug_info.append({
                        'time': t,
                        'missile_pos': missile_pos,
                        'smoke_center': smoke_center,
                        'is_blocked': is_blocked,
                        'missile_to_target': missile_to_target_dist,
                        'smoke_to_line': smoke_to_line_dist
                    })
        
        compute_time = time.time() - start_compute
        print(f"计算耗时: {compute_time:.3f}s")
        
        # 输出调试信息
        if len(debug_info) > 0 and obstruction_status.sum() == 0:
            print("\n调试信息（未检测到遮蔽）：")
            for info in debug_info[:5]:  # 只显示前5条
                print(f"时间 {info['time']:.1f}s: 烟幕到视线距离 {info['smoke_to_line']:.1f}m")
        
        # 统计遮蔽时长
        total_obstruction_time = np.sum(obstruction_status) * self.params['time_step']
        
        return time_points, obstruction_status, total_obstruction_time, explosion_pos, launch_pos
    
    def calculate_point_to_line_distance(self, point, line_start, line_end):
        """计算点到线段的最短距离"""
        # 线段向量
        line_vec = line_end - line_start
        point_vec = point - line_start
        
        # 投影长度
        line_len = np.linalg.norm(line_vec)
        if line_len == 0:
            return np.linalg.norm(point_vec)
        
        line_unitvec = line_vec / line_len
        proj_length = np.dot(point_vec, line_unitvec)
        
        # 限制在线段范围内
        proj_length = max(0.0, min(proj_length, line_len))
        
        # 最近点
        closest = line_start + proj_length * line_unitvec
        
        return np.linalg.norm(point - closest)
    
    def create_3d_visualization(self, explosion_pos, launch_pos):
        """创建3D场景可视化"""
        fig = plt.figure(figsize=(12, 10))
        ax = fig.add_subplot(111, projection='3d')
        
        # 绘制目标
        ax.scatter(*self.params['fake_target'], c='blue', s=200, marker='^', 
                   label='假目标', edgecolors='black', linewidth=2)
        ax.scatter(*self.params['real_target'], c='red', s=200, marker='o', 
                   label='真目标', edgecolors='black', linewidth=2)
        
        # 绘制导弹轨迹
        missile_trajectory = np.array([
            self.calculate_missile_position(t) 
            for t in np.linspace(0, self.total_time, 100)
        ])
        ax.plot(missile_trajectory[:, 0], missile_trajectory[:, 1], 
                missile_trajectory[:, 2], 'g--', linewidth=2, label='导弹轨迹')
        
        # 绘制无人机轨迹（等高度）
        uav_trajectory = np.array([
            self.calculate_uav_position(t)
            for t in np.linspace(0, 5, 50)
        ])
        ax.plot(uav_trajectory[:, 0], uav_trajectory[:, 1],
                uav_trajectory[:, 2], 'c-', linewidth=1.5, label='无人机轨迹')
        
        # 绘制起爆点
        ax.scatter(*explosion_pos, c='orange', s=300, marker='*', 
                   label='起爆点', edgecolors='black', linewidth=2)
        
        # 绘制投放点
        ax.scatter(*launch_pos, c='purple', s=200, marker='x', 
                   label='投放点', linewidth=3)
        
        # 绘制烟幕球
        u = np.linspace(0, 2 * np.pi, 20)
        v = np.linspace(0, np.pi, 20)
        x = self.params['smoke_radius'] * np.outer(np.cos(u), np.sin(v)) + explosion_pos[0]
        y = self.params['smoke_radius'] * np.outer(np.sin(u), np.sin(v)) + explosion_pos[1]
        z = self.params['smoke_radius'] * np.outer(np.ones(np.size(u)), np.cos(v)) + explosion_pos[2]
        ax.plot_surface(x, y, z, alpha=0.3, color='gray')
        
        ax.set_xlabel('X (米)', fontsize=12)
        ax.set_ylabel('Y (米)', fontsize=12)
        ax.set_zlabel('Z (米)', fontsize=12)
        ax.set_title('3D场景可视化', fontsize=14, fontweight='bold')
        ax.legend(loc='upper right', fontsize=10)
        ax.view_init(elev=20, azim=45)
        
        plt.tight_layout()
        filepath = os.path.join(self.output_dir, '01_3D场景可视化.png')
        plt.savefig(filepath, dpi=300, bbox_inches='tight')
        print(f"已保存: {filepath}")
        plt.show()
        return fig
    
    def visualize_results(self, time_points, obstruction_status, total_time, explosion_pos, launch_pos):
        """生成可视化结果"""
        print("\n生成可视化图表...")
        
        # 1. 3D可视化
        self.create_3d_visualization(explosion_pos, launch_pos)
        
        # 2. 遮蔽时间线
        fig = plt.figure(figsize=(12, 6))
        ax = plt.subplot(111)
        
        ax.fill_between(time_points, 0, obstruction_status, 
                         alpha=0.7, color='green', label='遮蔽有效')
        ax.set_xlabel('时间 (秒)', fontsize=12)
        ax.set_ylabel('遮蔽状态', fontsize=12)
        ax.set_title(f'遮蔽时间线分析 (总遮蔽时长: {total_time:.2f}秒)', 
                    fontsize=14, fontweight='bold')
        ax.set_ylim([-0.1, 1.1])
        ax.set_yticks([0, 1])
        ax.set_yticklabels(['无遮蔽', '有遮蔽'])
        ax.grid(True, alpha=0.3)
        
        # 标注关键时刻
        explosion_time = self.params['launch_time'] + self.params['explosion_delay']
        ax.axvline(x=explosion_time, color='red', linestyle='--', alpha=0.5, label='起爆时刻')
        ax.axvline(x=explosion_time + self.params['smoke_duration'], 
                   color='orange', linestyle='--', alpha=0.5, label='烟幕失效')
        
        ax.legend(loc='upper right', fontsize=10)
        plt.tight_layout()
        filepath = os.path.join(self.output_dir, '02_遮蔽时间线.png')
        plt.savefig(filepath, dpi=300, bbox_inches='tight')
        print(f"已保存: {filepath}")
        plt.show()

# 3. 模型调用
def main():
    """主函数"""
    
    # 创建仿真器
    simulator = SmokeScreenSimulator(PARAMS)
    
    # 执行仿真
    print("\n" + "="*40)
    print("开始仿真计算...")
    print("="*40)
    
    time_points, obstruction_status, total_obstruction_time, explosion_pos, launch_pos = simulator.simulate_obstruction()
    
    # 4. 结果输出
    print("\n" + "="*40)
    print("最终结果")
    print("="*40)
    print(f"✓ 有效遮蔽时长: {total_obstruction_time:.2f} 秒")
    print(f"✓ 遮蔽覆盖率: {total_obstruction_time/simulator.total_time*100:.1f}%")
    
    # 详细遮蔽时段
    obstruction_periods = []
    in_obstruction = False
    start_time = 0
    
    for i, status in enumerate(obstruction_status):
        if status == 1 and not in_obstruction:
            start_time = time_points[i]
            in_obstruction = True
        elif status == 0 and in_obstruction:
            end_time = time_points[i]
            obstruction_periods.append((start_time, end_time))
            in_obstruction = False
    
    if in_obstruction:  # 处理最后一个时段
        obstruction_periods.append((start_time, time_points[-1]))
    
    print(f"\n遮蔽时段详情:")
    if len(obstruction_periods) > 0:
        for i, (start, end) in enumerate(obstruction_periods):
            duration = end - start
            print(f"  第{i+1}段: {start:.1f}s - {end:.1f}s (持续{duration:.1f}s)")
    else:
        print("  无有效遮蔽时段")
        print("\n可能的原因：")
        print("  1. 烟幕位置不在导弹到真目标的视线上")
        print("  2. 烟幕半径太小（10米）")
        print("  3. 起爆时间或位置不合适")
    
    # 可视化
    simulator.visualize_results(time_points, obstruction_status, 
                                total_obstruction_time, explosion_pos, launch_pos)
    
    print("\n" + "="*60)
    print("问题1求解完成！")
    print(f"所有图片已保存至文件夹: {simulator.output_dir}")
    print("="*60)
    
    return total_obstruction_time

# 运行主程序
if __name__ == "__main__":
    result = main()